#include <pwm.h>
#include "p30fxxxx.h"
#include "stacktsk.h"
#include "tick.h"
#include <dsp.h>
#include "icmp.h"
#include <uart.h>
#include <ports.h>
#include "global.h"
#include "config.h"
#include "udp.h"


typedef union _EPP_VAL
{
        WORD Val;
        struct
        {
                BYTE LSB;
                BYTE MSB;
        }
        byte;
        BYTE v[2];
          struct{
			unsigned int b0: 1;
			unsigned int b1: 1;
			unsigned int b2: 1;
			unsigned int b3: 1;
			unsigned int b4: 1;
			unsigned int b5: 1;
			unsigned int b6: 1;
			unsigned int b7: 1;
			unsigned int b8: 1;
			unsigned int b9: 1;
			unsigned int b10: 1;
			unsigned int b11: 1;
			unsigned int b12: 1;
			unsigned int b13: 1;
			unsigned int b14: 1;
			unsigned int b15: 1;
        }bits;
		struct{
			unsigned int data0: 1;
			unsigned int data1: 1;
			unsigned int data2: 1;
			unsigned int data3: 1;
			unsigned int data4: 1;
			unsigned int data5: 1;
			unsigned int data6: 1;
			unsigned int data7: 1;
			unsigned int addrstrobe: 1;
			unsigned int datastrobe: 1;
			unsigned int interrupt: 1;
			unsigned int wait: 1;
			unsigned int write: 1;
			unsigned int reset: 1;
			unsigned int spare: 1;
			unsigned int spare2: 1;
		}epp;
		struct{
			unsigned int data0: 1;
			unsigned int data1: 1;
			unsigned int data2: 1;
			unsigned int data3: 1;
			unsigned int data4: 1;
			unsigned int data5: 1;
			unsigned int data6: 1;
			unsigned int data7: 1;
			unsigned int selectin: 1;
			unsigned int autofeed: 1;
			unsigned int ack: 1;
			unsigned int busy: 1;
			unsigned int strobe: 1;
			unsigned int init: 1;
			unsigned int paperout: 1;
			unsigned int select: 1;
		}spp;
} EPP_VAL;



#define DATASIZE 500
EPP_VAL __attribute__ ((aligned (16))) cdata[DATASIZE];
int readptr=0;
int writeptr=0;
int misread=0;
int overflow=0;
UDP_SOCKET motionsocket;
//Timer 3 variables
volatile unsigned int blinkcount;
volatile unsigned int blinklimit;
NODE_INFO debugnode;

//Prototypes
void encoderSetup();
void Timer3Init();
void testMode();
void PidTimerInit();
void reOpenMotion(BYTE close);
/*        
 ******Timer usage******
 1. Ethernet stack
 3. Blink LED timer
 */
//Programmer Configuration Bits
_FOSC( CSW_FSCM_OFF & EC_PLL8);
//_FWDT( WDT_ON & WDTPSA_64 & WDTPSB_4 ); //512 ms
_FWDT(WDT_OFF);
_FBORPOR( PBOR_ON & BORV_42 & MCLR_EN );
_FGS(CODE_PROT_OFF);

#define THIS_IS_STACK_APPLICATION
#define DEBUG_PORT 1111
#define BLINKINTERVAL 2000
#define TIMER3INTERVAL 10 
//Frequency Formulas
#define MISSING_PACKETS 25
#define REV_D 1


// -----------------------------------------------------------------------------
// Main Function
// -----------------------------------------------------------------------------
int main(void) {
	ClrWdt();
	//PORT setup
	TRISEbits.TRISE0=1;//DAC
	TRISEbits.TRISE5=1;//DAC
	TRISFbits.TRISF0=1;//OUT1
	TRISFbits.TRISF1=1;//OUT2
	TRISEbits.TRISE4=1;//OUT3
	TRISEbits.TRISE3=1;//OUT4
	TRISEbits.TRISE2=1;//OUT5
	TRISEbits.TRISE8=1;//OUT6
	TRISEbits.TRISE1=1;//OUT7
	TRISBbits.TRISB0=1;//OUT8
	TRISBbits.TRISB1=0;//LED1
	TRISBbits.TRISB2=0;//LED2
	TRISFbits.TRISF5=1;//UART
	TRISDbits.TRISD0=1;//RD0 C
	TRISDbits.TRISD1=1;//RD1 D
	//Inputs
	TRISBbits.TRISB8=1;//in1
	TRISBbits.TRISB7=1;//in3
	TRISBbits.TRISB6=1;//in2
	TRISCbits.TRISC13=1;//alarm a
	TRISCbits.TRISC14=1;//alarm b
	TRISBbits.TRISB3=1;//qei z
	TRISBbits.TRISB4=1;//qei a
	TRISBbits.TRISB5=1;//qei b
	//Turn on LED status flags
	LED1=1;
	Nop();
	Nop();
	Nop();
	LED2=1;
	ClrWdt();
	ADPCFG=0xFFFF;//all digital
	//load all eeprom data and initalize globals
	loadConfig();
	ClrWdt();
	StackInit();
	//indicate ethernet success, via led
	LED1=0;
	//initalize daemons
	modbusInit();
	configInit();//config daemon
	ClrWdt();
	//zero buffer
	int i=0;
	for(i=0;i<DATASIZE;i++){
		cdata[i].Val=0;
	}
	readptr=0;
	writeptr=1;
	ClrWdt();
	AnnounceIP();//required for stack to work without a ping
	ClrWdt();
	blinklimit=(unsigned int)(BLINKINTERVAL/((float)TIMER3INTERVAL)); //how many TIMER3INTERVAL periods
	blinkcount=0;
	Timer3Init();
	openMotion();
	//enable interrupts for parallel capture
	ConfigIntCN(CHANGE_INT_ON & CHANGE_INT_PRI_5 & 0xFF040003);
	EnableCN1;
	CNEN2bits.CN18IE = 1;
	//Main Loop
	while (1) {
		ClrWdt();//kick the dog
		//flash LED2 to indicate software running
		if (blinkcount>blinklimit) {
			LED2=!LED2;
			blinkcount=0;	
		}
		sendData();
		//run stack and daemons
		StackTask();
		modbusServer();
		configServer();
	}
}//main


void sendData(){
	while (!UDPIsPutReady(motionsocket));
	int read=0;
	asm("DISI #50");
	if(writeptr<readptr){
		read=DATASIZE-readptr+writeptr;
	}else if(writeptr>readptr){
		read=writeptr-readptr-1;
	}
	if(read==0){
		return;
	}
	int i;
	for(i=0;i<read;i++){
		readptr++;
		EPP_VAL data=cdata[readptr];
		UDPPut(data.v[0]);
		UDPPut(data.v[1]);
		cdata[readptr].Val=0xCCCC;
		asm("DISI #20");
		if(readptr==DATASIZE){
			readptr=-1;
		}
	}
	UDPFlush();
}


void openMotion() {
	//setup debug udp output
	debugnode.MACAddr.v[0] = 0x00;
	debugnode.MACAddr.v[1] = 0x08;
	debugnode.MACAddr.v[2] = 0x54;
	debugnode.MACAddr.v[3] = 0xB0;
	debugnode.MACAddr.v[4] = 0xEC;
	debugnode.MACAddr.v[5] = 0xC0;
	debugnode.IPAddr.v[0]=63;
	debugnode.IPAddr.v[1]=147;
	debugnode.IPAddr.v[2]=251;
	debugnode.IPAddr.v[3]=73;
	motionsocket=UDPOpen(DEBUG_PORT,&debugnode,DEBUG_PORT);
}

EPP_VAL loadData(){
	EPP_VAL data;
	data.Val=0;
	data.epp.data0=PORTFbits.RF1;//OUT2
	data.epp.data1=PORTEbits.RE4;//OUT3
	data.epp.data2=PORTEbits.RE3;//OUT4
	data.epp.data3=PORTEbits.RE2;//OUT5
	data.epp.data4=PORTEbits.RE8;//OUT6
	data.epp.data5=PORTEbits.RE1;//OUT7
	data.epp.data6=PORTBbits.RB0;//OUT8
	data.epp.data7=PORTDbits.RD0;//RD0 C
	data.epp.addrstrobe=PORTFbits.RF5; //UTX CN18
	data.epp.datastrobe=PORTCbits.RC13;//alarm a CN1
	data.epp.write=PORTCbits.RC14;//OUT1
	data.epp.spare=PORTEbits.RE5;//FWD_REV
	data.epp.spare2=PORTBbits.RB8;//in1
//data.epp.spare3=PORTBbits.RB6;//in2
	data.epp.wait=PORTEbits.RE0;//DAC
	data.epp.reset=PORTBbits.RB7;//in3
	data.epp.interrupt=PORTDbits.RD1;//RD1 D
	return data;
}

void addData(EPP_VAL eppdata){
	int free=0;
	if(writeptr<readptr){
		free=readptr-writeptr-1;
	}else if(writeptr>readptr){
		free=DATASIZE-(writeptr-readptr)-1;
	}
	if(free==0){
		LED1==1;
		LED2==1;
		overflow++;
		//lose data
		return;
	}
	cdata[writeptr]=eppdata;
	writeptr++;
	if(writeptr==DATASIZE){
		writeptr=0;
	}
}



void _ISR _T3Interrupt(void) {
	//reset timer
	TMR3=0x0000;
	blinkcount++;
	IFS0bits.T3IF = 0;

}

//Timer 3 fired every TIMER3INTERVAL
void Timer3Init() {
	T3CONbits.TON=0;
	//16-bit, internal timer
	T3CONbits.TCKPS=0b11;//postscaler
	TMR3=0x0000;
	PR3 = (Fcy/256)*(TIMER3INTERVAL)*0.001;
	IFS0bits.T3IF = 0; //clear interrupt flag
	IPC1bits.T3IP = 2; // set interrupt priority 
	IEC0bits.T3IE = 1; // enable timer 1 interrupt
	T3CONbits.TON = 1; //start the timer
}

EPP_VAL oldcnint;
void _ISR _CNInterrupt(void) {
	EPP_VAL data=loadData();
	/*   NEGOTIATION CAPTURE  */
/*	if(data.spp.selectin==1 && data.spp.autofeed==0){
		//negotiation started
		addData(data);
		//wait for support 1284 mode
		while(data.spp.ack!=1 && data.spp.select!=0 && data.spp.paperout!=0){
			data=loadData();
		}
		addData(data);
		while(data.spp.strobe!=1){
			data=loadData();
		}
		addData(data);
		//host recognizes a 1284 device
		while(data.spp.strobe!=0 && data.spp.autofeed!=0){
			data=loadData();
		}		
		addData(data);
		while(data.spp.ack!=0){
			data=loadData();
		}
		addData(data);
	}///*/
	/*   EPP CAPTURE  */
	//writing data
	if(data.epp.write==0){
		if(data.epp.addrstrobe==0){
			//writing address
			addData(data);
		}else if(data.epp.datastrobe==0){
			//writing data
			addData(data);
		}else{
			misread++;
			addData(data);
		}
	}else{
		//reading data
		if(oldcnint.epp.addrstrobe==1 && data.epp.addrstrobe==0){
			//reading address
			addData(data);
		}else if(oldcnint.epp.datastrobe==1 && data.epp.datastrobe==0){
			//reading data
			addData(data);
		}else{
			misread++;
			addData(data);
		}
	}//*/
	oldcnint=data;
	IFS0bits.CNIF=0; //clear int flag
}


void _ISR _DefaultInterrupt(void)
{
	while(1) {
		Nop();
		Nop();
		Nop();
		LED2=0; 
		Nop();
		Nop();
		LED2=1;
	}
}

